#include <hal_move.h>
#include "hal_position.h"
#include "hal_errorcode.h"
#include "cfg_drive.h"
#include "spi.h"
#include <common.h>
#include "sys_log.h"
#include "tim.h"
#include "math.h"
#include "cfg_drive.h"

// 定义 定时器计转动步数
static uint16_t cnt = 0;
// 定义 电机S曲线结构体
static MotorSCurveSt MotorSCurve = {0};
// 定义 定时器重载值数组
static uint16_t Tim_Period[CURVE_S_PERIOD_SIZE] = {[0 ... CURVE_S_PERIOD_SIZE - 1] = CURVE_S_PERIOD_INIT};
// 定义 四个步序高低电平
const uint8_t MotrCtrlStep[4][4] = {{1, 0, 0, 1}, {1, 0, 1, 0}, {0, 1, 1, 0}, {0, 1, 0, 1}};
// 定义 当前组别电机号以及电流最大值
MotorMoveSt MotorMove = {0};

uint8_t g_current_error_flag = 0;

static uint8_t hal_PWMAdjust();
static void hal_CurveSInit(uint16_t *pbuff, uint16_t len);
static void hal_PWMCaculate(uint16_t PWM_STEP);

static void hal_MotorCurrentCtrl(uint8_t groupid, uint8_t motor_num);

static void hal_MotorMoveDeal();
static void hal_MotorStepCtrl(uint8_t groupid, uint8_t motor_num);
static void hal_MotorStepGpoiWrite(uint8_t groupid, uint8_t step_state);

static uint16_t hal_findNonZeroMin(uint16_t *arr, uint16_t len);

// 对重载值保护，不为0有效
#define __HAL_TIM_SET_AUTORELOAD__(t, v)                                                                                                                                                               \
    do                                                                                                                                                                                                 \
    {                                                                                                                                                                                                  \
        if (v)                                                                                                                                                                                         \
            __HAL_TIM_SET_AUTORELOAD(t, v);                                                                                                                                                            \
    }                                                                                                                                                                                                  \
    while (0)

// 定时器控制电机转动
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &htim6)
    {
        // 电机转动事件
        hal_MotorMoveDeal(); // 77,78,79us
    }

    // 四个电机最多运行 4000步，超过阈值报错误码, 一般也不会遇到
    if (cnt++ >= (MOTOR_RUN_MAX_STEP * 1.2))
    {
        for (int i = 1; i <= 4; i++)
        {
            hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Timer_Abnormal_Flag, i, ABNORMAL_STATE);
        }
        HAL_TIM_Base_Stop_IT(&htim6);
        cnt = 0;
        memset(&MotorSCurve, 0, sizeof(MotorSCurveSt));
        memset(&Tim_Period, CURVE_S_PERIOD_INIT, sizeof(Tim_Period));
        hal_MotorStepCtrlDriveSet(DRIVE_STATUES_CLOSE);
        // 定时器已经异常，所以直接stop,然后打印输出
        hal_MotorMoveStartFlagSet(MOVE_STATUES_STOP);
        
        SLOG_I("motor HAL_TIM_PeriodElapsedCallback outlimit cnt \t\r\n");
    }
}

// S曲线结构体赋值
static void hal_PWMCaculate(uint16_t PWM_STEP)
{
    if (!PWM_STEP)
    {
        return;
    }

    MotorSCurve.PWMcount = 0;
    MotorSCurve.PWMneed  = PWM_STEP;
    MotorSCurve.Count    = 0;
    MotorSCurve.Status   = SPEED_MODEL_INCREASE;
    MotorSCurve.CountMax = ceil(PWM_STEP / 2.0);

    hal_CurveSInit(Tim_Period, MotorSCurve.CountMax);
}

// 计算转动步进范围内的S曲线重载值数组
static void hal_CurveSInit(uint16_t *pbuff, uint16_t len)
{
    uint16_t i;
    float delt = FRE_MAX - FRE_MIN;
    float deno;
    float melo;
    float fre;

    for (i = 0; i < len; i++)
    {
        melo     = FRE_FLEXIBLE * (i - len / 2) / (len / 2);
        deno     = 1.0f / (1 + expf(-melo));
        fre      = delt * deno + FRE_MIN;
        pbuff[i] = (uint16_t)(TIMER_DIV_FRE / fre);
    }
}

// 根据当前步数计算下一个转动步数的定时器重载值，同时也判断加速还是减速
static uint8_t hal_PWMAdjust()
{
    switch (MotorSCurve.Status)
    {
    /*加速*/
    case SPEED_MODEL_INCREASE:
        if (MotorSCurve.Count < MotorSCurve.CountMax)
        {
            __HAL_TIM_SET_AUTORELOAD__(&htim6, Tim_Period[MotorSCurve.Count]); //计算下一个PWM的周期  别忘了改
            MotorSCurve.Count++;                                               //加速次数
        }
        else
        {
            MotorSCurve.Status = SPEED_MODEL_DECREASE; // 直接减速没有匀速阶段
            MotorSCurve.Count--;
        }
        break;
    /*减速*/
    case SPEED_MODEL_DECREASE:
        if (MotorSCurve.Count > 0)
        {
            __HAL_TIM_SET_AUTORELOAD__(&htim6, Tim_Period[MotorSCurve.Count]); //计算下一个PWM的周期
            MotorSCurve.Count--;
        }
        if (MotorSCurve.PWMcount > MotorSCurve.PWMneed)
        {
            // 意味着转动需要结束，此时stepnum里面已经为0
            __HAL_TIM_SET_AUTORELOAD__(&htim6, PRESCALER); //恢复到初始1s一次的分频
            return 1;
        }
        break;
    default:
        break;
    }

    return 0;
}

void hal_MotorMoveDeal()
{
    static uint16_t i = 0;

    uint8_t motor_num = 0;
    uint8_t statues   = MOTOR_MAX_ERROR_CHANNEL;

    // LED闪烁计数
    if (i++ > TIMER_LED_COUNT)
    {
        HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
        i = 0;
    }

    // 转动步数++
    MotorSCurve.PWMcount++;

    // 该轮N组电机正常转动结束
    if (hal_PWMAdjust() == 1)
    {
        for (uint8_t groupid = 1; groupid <= MOTOR_MAX_ERROR_CHANNEL; groupid++)
        {
            if (hal_MotorNumRead(groupid))
            {
                // 该轮N组电机正常转动结束
                hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Run_Abnormal_Flag, groupid, NORMAL_STATE);
            }
        }
        // N个电机一轮正常转动结束
        HAL_TIM_Base_Stop_IT(&htim6);
        cnt = 0;
        memset(&MotorSCurve, 0, sizeof(MotorSCurveSt));
        memset(&Tim_Period, CURVE_S_PERIOD_INIT, sizeof(Tim_Period));
        hal_MotorStepCtrlDriveSet(DRIVE_STATUES_CLOSE);
        // 只是一轮并不是四轮，所以要重新开始
        hal_MotorMoveStartFlagSet(MOVE_STATUES_START);
        SLOG_I("motor one round of normal move success \t\r\n");
    }
    else
    {
        // 电机仍在转动过程中
        for (uint8_t groupid = 1; groupid <= MOTOR_MAX_ERROR_CHANNEL; groupid++)
        {
            motor_num = hal_MotorNumRead(groupid);

            if (motor_num != 0)
            {
                /*
                1、运行正常到0
                2、堵转设置为0
                */
                if (hal_StepNumberRead(groupid, motor_num) != 0)
                {
                    // 当前组别正有电机在转动
                    hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Run_Abnormal_Flag, groupid, ABNORMAL_STATE);

                    // 先判断上一步电流,且前几步和后几步都不测量，以避免误报堵转
                    if (cnt > MOTOR_NO_CURRENT_TEST_STEP && hal_StepNumberRead(groupid, motor_num) > MOTOR_NO_CURRENT_TEST_STEP)
                    {
                        hal_MotorCurrentCtrl(groupid, motor_num);
                    }
                    // 有电机且有步进需要转动
                    statues--;
                    // 单步数控制
                    hal_MotorStepCtrl(groupid, motor_num);
                }
                else
                {
                    // stepnumber为0要么正常结束要么堵转置0
                    hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Run_Abnormal_Flag, groupid, NORMAL_STATE);
                }
            }
        }

        // N个电机该轮转动都堵转或者四轮正常转动结束
        if (statues == MOTOR_MAX_ERROR_CHANNEL)
        {
            HAL_TIM_Base_Stop_IT(&htim6);
            cnt = 0;
            memset(&MotorSCurve, 0, sizeof(MotorSCurveSt));
            memset(&Tim_Period, CURVE_S_PERIOD_INIT, sizeof(Tim_Period));
            hal_MotorStepCtrlDriveSet(DRIVE_STATUES_CLOSE);
            hal_MotorMoveStartFlagSet(MOVE_STATUES_STOP);
            HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
            SLOG_I("motor four round of normal move over or locked \t\r\n");
        }
    }
}

static void hal_MotorCurrentCtrl(uint8_t groupid, uint8_t motor_num)
{
    // 获取堵转电流
    float current = HAL_AD7689CurrentRead(groupid, motor_num);
    if (current > MotorMove.current[groupid - 1])
    {
        MotorMove.current[groupid - 1] = current;
    }

    // 电流超过阈值
    if (current > MOTOR_CURRENT_LIMIT)
    {
        hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Current_Abnormal_Flag, groupid, ABNORMAL_STATE);
        hal_StepNumberWrite(groupid, motor_num, 0);
        // 定时器中最好不要加打印，以标志位的形式放在while前台打印！！！
        // 因为转动结束才会进行下一组电机转动，因此前台的打印不会滞后
        g_current_error_flag = 1;
    }
    else
    {
        // 如果不进行错误码刷新则只能下一组电机同样序号转动第一次电流不超阈值时会置正常
        // 因为上位机会刷新因此这条代码不起作用
        hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_Current_Abnormal_Flag, groupid, NORMAL_STATE);
    }
}

static void hal_MotorStepCtrl(uint8_t groupid, uint8_t motor_num)
{
    // 获取电机转动过程中的方向、步进、步序、位置
    uint16_t direction  = hal_DirectionRead(groupid, motor_num);
    int16_t step_number = hal_StepNumberRead(groupid, motor_num);
    uint16_t step_state = hal_StepStateRead(groupid, motor_num);
    uint16_t loaction   = hal_PositionRead(groupid, motor_num);

    //正转
    if (direction == DIRECTION_FORWARD)
    {
        if (step_state > 1)
            step_state -= 1;
        else
            step_state = 4;

        hal_MotorStepGpoiWrite(groupid, step_state);
        loaction++;
    }
    //反转
    else if (direction == DIRECTION_BACKWARD)
    {
        if (step_state < 4)
            step_state += 1;
        else
            step_state = 1;

        hal_MotorStepGpoiWrite(groupid, step_state);
        loaction--;
    }

    if (--step_number < 0)
    {
        step_number = 0;
    }
    // 更新步序
    hal_StepStateWrite(groupid, motor_num, step_state);
    // 更新步进步数 - 1
    hal_StepNumberWrite(groupid, motor_num, step_number);
    // 更新存储到eeprom的位置
    hal_PositionWrite(groupid, motor_num, loaction);
}

static void hal_MotorStepGpoiWrite(uint8_t groupid, uint8_t step_state)
{
    // 根据步序设置电平
    MOTOR_GPIO_WRITE_PIN(groupid, IN_CH1, MotrCtrlStep[step_state - 1][0]);
    MOTOR_GPIO_WRITE_PIN(groupid, IN_CH2, MotrCtrlStep[step_state - 1][1]);
    MOTOR_GPIO_WRITE_PIN(groupid, IN_CH3, MotrCtrlStep[step_state - 1][2]);
    MOTOR_GPIO_WRITE_PIN(groupid, IN_CH4, MotrCtrlStep[step_state - 1][3]);
}

void HAL_AD7689_Init(uint8_t groupid, uint8_t motor_num)
{
    HAL_StatusTypeDef status_transmitreceive;

    uint16_t TempReg     = 0;
    uint16_t AD7689_WCfg = (uint16_t)AD7689_WCfg(motor_num - 1);
    AD7689_WCfg <<= 2;
    float RealData = 0;

    SPI_HandleTypeDef *hspix = (groupid == GROUP_1) ? &hspi1 : (groupid == GROUP_2) ? &hspi2 : (groupid == GROUP_3) ? &hspi3 : &hspi4;

    for (int i = 0; i < 3; i++)
    {
        TempReg = 0;

        MOTOR_GPIO_WRITE_PIN(groupid, ADC_NSS, GPIO_PIN_SET);
        MOTOR_GPIO_WRITE_PIN(groupid, ADC_NSS, GPIO_PIN_RESET);
        status_transmitreceive = HAL_SPI_TransmitReceive(hspix, (uint8_t *)&AD7689_WCfg, (uint8_t *)&TempReg, 1, AD7689_SPI_DELAY_TIME_LIMIT);
        MOTOR_GPIO_WRITE_PIN(groupid, ADC_NSS, GPIO_PIN_SET);
        nop_delay_us(AD7689_SAMP_DELAY);
    }

    if (status_transmitreceive != HAL_OK)
    {
        hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_ADC7689_Abnormal_Flag, groupid, ABNORMAL_STATE);
    }
}

float HAL_AD7689CurrentRead(uint8_t groupid, uint8_t motor_num)
{
    HAL_StatusTypeDef status_transmitreceive;

    uint16_t TempReg     = 0;
    uint16_t AD7689_WCfg = (uint16_t)AD7689_WCfg(motor_num - 1);
    AD7689_WCfg <<= 2;
    float RealData = 0;

    SPI_HandleTypeDef *hspix = (groupid == GROUP_1) ? &hspi1 : (groupid == GROUP_2) ? &hspi2 : (groupid == GROUP_3) ? &hspi3 : &hspi4;

    TempReg = 0;

    MOTOR_GPIO_WRITE_PIN(groupid, ADC_NSS, GPIO_PIN_SET);
    MOTOR_GPIO_WRITE_PIN(groupid, ADC_NSS, GPIO_PIN_RESET);
    status_transmitreceive = HAL_SPI_TransmitReceive(hspix, (uint8_t *)&AD7689_WCfg, (uint8_t *)&TempReg, 1, AD7689_SPI_DELAY_TIME_LIMIT);
    MOTOR_GPIO_WRITE_PIN(groupid, ADC_NSS, GPIO_PIN_SET);
    nop_delay_us(AD7689_SAMP_DELAY);

    // *1000 变成mA
    RealData = (float)TempReg / AD7689_MOTOR_PARA;

    if (status_transmitreceive != HAL_OK)
    {
        hal_ErrorCodeWrite(ERRORCODE_TYPE_MOTOR, m_ADC7689_Abnormal_Flag, groupid, ABNORMAL_STATE);
    }

    return RealData;
}

void hal_MotorMoveStart()
{
    uint16_t step_num[MOTOR_MAX_ERROR_CHANNEL] = {0};

    for (int i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
    {
        step_num[i - 1] = hal_StepNumberRead(i, hal_MotorNumRead(i));
    }

    // 找到四组中的非0最小值
    uint16_t MinStepNum = hal_findNonZeroMin(step_num, MOTOR_MAX_ERROR_CHANNEL);
    SLOG_I("motor hal_MotorMoveStart MinStepNum: %d, motor step_num: %d, %d, %d, %d \t\r\n", MinStepNum, step_num[0], step_num[1], step_num[2], step_num[3]);

    // step_num 全部为0 - 要么全部堵转/要么全部完成
    if (MinStepNum == 0)
    {
        // 先关闭定时器
        HAL_TIM_Base_Stop_IT(&htim6);
        // 再关闭所有驱动
        hal_MotorStepCtrlDriveSet(DRIVE_STATUES_CLOSE);
        // 转动状态为停止
        hal_MotorMoveStartFlagSet(MOVE_STATUES_STOP);
        // 转动闪烁灯置灭
        HAL_GPIO_WritePin(LED1_GPIO_Port, LED1_Pin, GPIO_PIN_SET);
        // // 当前组的堵转电流最大值清零
        // memset(max_cur, 0, sizeof(max_cur));
    }
    else
    {
        // 开始转动前先把定时器关闭再打开
        HAL_TIM_Base_Stop_IT(&htim6);
        // 计算S曲线重载值
        hal_PWMCaculate(MinStepNum);
        // 打开所有驱动
        hal_MotorStepCtrlDriveSet(DRIVE_STATUES_OPEN);
        // 转动状态为无
        hal_MotorMoveStartFlagSet(MOVE_STATUES_NONE);
        // 最后打开定时器，开始由定时器控制电机转动
        HAL_TIM_Base_Start_IT(&htim6);
    }
}

void hal_MotorStepCtrlDriveSet(DriveStatuesEn DriveFlag)
{
    uint8_t motor_num = 0;

    for (int i = 1; i <= MOTOR_MAX_ERROR_CHANNEL; i++)
    {
        motor_num = hal_MotorNumRead(i);

        if (motor_num != 0)
        {
            // sleep引脚拉高
            MOTOR_GPIO_WRITE_PIN(i, SLP, (DriveFlag == DRIVE_STATUES_OPEN) ? GPIO_PIN_SET : GPIO_PIN_RESET);
            // en引脚拉高
            MOTOR_GPIO_WRITE_PIN(i, EN_CTR, (DriveFlag == DRIVE_STATUES_OPEN) ? GPIO_PIN_RESET : GPIO_PIN_SET);
            // pwr引脚拉高
            MOTOR_GPIO_WRITE_PIN(i, PWR_ON, (DriveFlag == DRIVE_STATUES_OPEN) ? GPIO_PIN_SET : GPIO_PIN_RESET);

            // slp_slc引脚根据电机编号拉低/拉高
            if (DriveFlag == DRIVE_STATUES_OPEN)
            {
                MOTOR_GPIO_WRITE_PIN(i, SLP_SLC1, ((motor_num - 1) & 0x01) == 0 ? GPIO_PIN_RESET : GPIO_PIN_SET);
                MOTOR_GPIO_WRITE_PIN(i, SLP_SLC2, ((motor_num - 1) & 0x02) == 0 ? GPIO_PIN_RESET : GPIO_PIN_SET);
                MOTOR_GPIO_WRITE_PIN(i, SLP_SLC3, ((motor_num - 1) & 0x04) == 0 ? GPIO_PIN_RESET : GPIO_PIN_SET);
            }
        }
    }
}

static uint16_t hal_findNonZeroMin(uint16_t *arr, uint16_t len)
{
    // 假设数组至少包含一个不为零的元素
    uint16_t temp = MOTOR_RUN_MAX_STEP * 2;

    // 遍历数组，找到不为零的最小值
    for (int i = 0; i < len; i++)
    {
        if (arr[i] != 0 && arr[i] < temp)
        {
            temp = arr[i];
        }
    }

    if (temp == MOTOR_RUN_MAX_STEP * 2)
    {
        return 0;
    }

    return temp;
}
